#include <ros/ros.h>
#include <fstream>
#include "sensor_msgs/Imu.h"
#include "tf/transform_datatypes.h"
#include <tf/transform_listener.h>
#include "geometry_msgs/PoseStamped.h"
#include <nav_msgs/Odometry.h>
#include "sensor_msgs/LaserScan.h"
#include <visualization_msgs/Marker.h>
#include <tf/transform_listener.h>

#define TURN_ANGLE 15 //degrees
const double thresh_ang = TURN_ANGLE * (3.1416/180.0);

double time_last, yaw_last = 0, delta_t, vel_linear, vel_angular, accel, curr_yaw;
double r,p,y=0, initial_yaw , temp_yaw,current_yaw, end_park_angle, ox,oy,lx,ly, turned_angle, start_park_angle, initial_x, initial_y; tf::Quaternion quat;
bool is_first = 1; bool first_time_rec = 0, end_time_rec = 0;
int iter = 0; bool counting = 0;
std::ofstream myfile;

bool first_pose_rec=0, end_pose_rec=0;
geometry_msgs::Pose start_pose, end_pose;

double  cma_yaw=0,sig_yaw=0,std_dev_yaw,cma_lin_v=0,sig_lin_v=0,std_dev_lin_v,dx,dy,dt,lin_v, dx1,dy1,dt1;
int lin_v_c=0, yaw_c=0; bool prev_state_save=1; geometry_msgs::PoseStamped prev_pose;

ros::Time start_time, end_time;
ros::Time now;
std::vector<double> yaws,vels;

sensor_msgs::LaserScan laser_scan;
float min_range;


using namespace std;
visualization_msgs::Marker line;
ros::Publisher marker_pub;

double hypotenuse(double xa, double ya, double xb, double yb)
{
  double dx = xb-xa; double dy = yb-ya;
  return sqrt(dx*dx + dy*dy);
}


// return minimum distance between two line segments defined by 4 points
void DistanceFromSegment(double p1x, double p1y, double p2x, double p2y ,
			 double p3x, double p3y, double p4x, double p4y,double &distanceSegment)

{
    double EPS = 0.00000001;

    double ux = p1x - p2x;
    double uy = p1y - p2y;
    double vx = p3x - p4x;
    double vy = p3y - p4y;
    double wx = p2x - p4x;
    double wy = p2y - p4y;

    double a= ux*ux + uy*uy;
    double b= ux*vx + uy*vy;
    double c= vx*vx + vy*vy;
    double d= ux*wx + uy*wy;
    double e= vx*wx + vy*wy;

    double D = a*c - b*b;
    double sD = D;
    double tD = D;

    double sc;
    double sN;
    double tN;
    double tc;

    if (D < EPS)
    {
        sN = 0.0;
        sD = 1.0;
        tN = e;
        tD = c;
    }
    else
    {
        sN = (b * e - c * d);
        tN = (a * e - b * d);
        if (sN < 0.0)
        {
            sN = 0.0;
            tN = e;
            tD = c;
        }
        else if (sN > sD)
        {
            sN = sD;
            tN = e + b;
            tD = c;
        }
    }

    if (tN < 0.0)
    {
        tN = 0.0;

        if (-d < 0.0)
            sN = 0.0;
        else if (-d > a)
            sN = sD;
        else
        {
            sN = -d;
            sD = a;
        }
    }
    else if (tN > tD)
    {
        tN = tD;
        if ((-d + b) < 0.0)
            sN = 0;
        else if ((-d + b) > a)
            sN = sD;
        else
        {
            sN = (-d + b);
            sD = a;
        }
    }

    if (fabs(sN) < EPS) {
        sc = 0.0;
                        }
        else { sc = sN / sD;
                }

        if (fabs(tN) < EPS){
            tc = 0.0;
                    }
            else  {
                tc = tN / tD;
               }

    	double   dPx = wx + (sc * ux) - (tc * vx);  // = S1(sc) - S2(tc)
    	double   dPy = wy + (sc * uy) - (tc * vy);  // = S1(sc) - S2(tc)

distanceSegment = sqrt(dPx*dPx + dPy*dPy);

return;
}

void imuCallback(const sensor_msgs::ImuConstPtr& msg)
{
  tf::quaternionMsgToTF(msg->orientation, quat);
  tf::Matrix3x3(quat).getRPY(r,p,y); 
 
// Initial yaw record
  if(is_first)
  {
	initial_yaw = y; //ROS_INFO("Datum yaw %f deg", y*57.3);
	is_first = 0;
  }
  
// Start parking when the user start turning in order to park
if(fabs(y-initial_yaw) >= thresh_ang && !first_time_rec)
	{
		//start_park_angle = y;	
		start_time = ros::Time::now(); //ROS_INFO("Start parking recorded on angle %f", start_park_angle*57.3);
		first_time_rec = 1;
	}
 
// End parking when user stop drivng and get the chair orientation
if(end_pose_rec && !end_time_rec)
  	{
	end_park_angle= y;
	//ROS_INFO("End parking recorded on angle %f", end_park_angle*57.3);
	//end_time = ros::Time::now();
	end_time_rec = 1; 
  	}

//Standard deviation for the orientation
if(!is_first && !end_time_rec)
  {
	cma_yaw = (y + yaw_c*cma_yaw) / (double)(yaw_c+1); yaw_c++;
	yaws.push_back(y);
  }
}


void poseCallback(const geometry_msgs::PoseStampedConstPtr& msg)
{
// Get the pose when start parking
if(first_time_rec && !first_pose_rec)
  {
	prev_pose.pose = start_pose = msg->pose;
	prev_state_save = 0;
	prev_pose.header.stamp = ros::Time::now();
	first_pose_rec = 1; ROS_INFO("Start pose recorded");
  }

  if(!prev_state_save && first_pose_rec) prev_state_save = 1;
  if(prev_state_save && first_pose_rec && !end_time_rec)
  {
	now = ros::Time::now(); 
	dx = msg->pose.position.x-prev_pose.pose.position.x;  
	dy = msg->pose.position.y-prev_pose.pose.position.y;
	dt = (now-prev_pose.header.stamp).toSec();
	if(dt>0.0)
	{
		lin_v = sqrt(dx*dx+dy*dy)/dt; 
		cma_lin_v = (lin_v + lin_v_c*cma_lin_v) / (double)(lin_v_c+1); lin_v_c++;
		vels.push_back(lin_v);

		prev_pose.header.stamp = now; prev_pose.pose = msg->pose;
		//Stop parking when velocity is zero, get the end pose
		if(lin_v < 0.1  && !end_pose_rec)
		{
			end_pose = msg->pose; 
			end_time = ros::Time::now();
			end_pose_rec = 1; ROS_INFO("End pose recorded");
  		}


	}
  }
}

void scanCallback(const sensor_msgs::LaserScan::ConstPtr& msg)
{

    std::vector<float> laser;
    laser = msg->ranges;

    int size_laser = laser.size();
    for (int i=0;i<size_laser;i++){
        if (laser[i] < 0.01){
            laser[i] = 99999;
        }
        if (laser[i] > 45){
            laser[i] = 99999;
        }
    }

    min_range = 2;
    int index_min;
    for (int i=0;i<size_laser;i++){
        if (laser[i] < min_range){
            min_range = laser[i];
            index_min = i;
	    //ROS_INFO("Minimum Range = %f", min_range);
        }
    }

    for (int j=0;j<size_laser;j++){
        if (laser[j] > min_range + 0.5){
            laser[j] = 0;
        }
    }

    laser_scan = *msg;
    laser_scan.ranges.clear();
    laser_scan.ranges = laser;

}


int main(int argc, char **argv)
{

  ros::init(argc, argv, "my_tf_listener");	
  ros::NodeHandle node;
  ros::Subscriber sub2 = node.subscribe("slam_out_pose", 1, poseCallback);
  ros::Subscriber sub =  node.subscribe("raw_imu_throttle", 1, imuCallback);
  ros::Subscriber sub3 = node.subscribe("scan_throttle", 1, scanCallback);
  //ros::Publisher laser_pub = node.advertise<sensor_msgs::LaserScan>("closest_points", 1);


			//Visualize the Table
		marker_pub = node.advertise<visualization_msgs::Marker>("BED", 10);
		line.header.frame_id = "map";
	    	line.header.stamp = ros::Time::now();
	    	line.ns = "BED";
	    	line.type = visualization_msgs::Marker::LINE_STRIP;
	    	line.action = visualization_msgs::Marker::ADD;
	    	line.id = 0;

		line.color.b = 2.0;
	    	line.color.a = 2.0;
	    	line.scale.x = 0.1;	//Sets the width of the LINE_STRIP
	   	line.scale.y = 0.1;	//Ignored if marker type is LINE_STRIP
		line.points.resize(2);

		tf::TransformListener listener;
  		ros::Rate rate(10.0);
		ros::Duration wait(7); wait.sleep();
		int speed_count = 0; double vel,yaw, xlast = 0 ,ylast = 0;
	
	while (node.ok())
	{

			//Speed calculation
			tf::StampedTransform transform2;
			if(speed_count==9)
				{
    				try{
      					listener.lookupTransform("/map", "/scanmatcher_frame",  
                               		ros::Time(0), transform2);
    				   }
    				catch (tf::TransformException ex){};
    				vel = hypotenuse(xlast,ylast, transform2.getOrigin().x(),transform2.getOrigin().y());
				if(vel<0.08)
					{
					yaw = tf::getYaw(transform2.getRotation());
					}

				speed_count = 0;
				xlast = transform2.getOrigin().x(); ylast = transform2.getOrigin().y();
				}
			else speed_count++;
				//Get the 4 corners of the wheelchair		

			tf::StampedTransform transform;
    			try{
      				listener.lookupTransform("/map", "/corner1",  
                               	ros::Time(0), transform);
    			    }
    			catch (tf::TransformException ex){};
			double x_vect_a = transform.getOrigin().x();
			double y_vect_a = transform.getOrigin().y();
			
			try{
      				listener.lookupTransform("/map", "/corner2",  
                               	ros::Time(0), transform);
    			    }
    			catch (tf::TransformException ex){};
    			
			double x_vect_b = transform.getOrigin().x();
			double y_vect_b = transform.getOrigin().y();
			
			try{
      				listener.lookupTransform("/map", "/corner3",  
                               	ros::Time(0), transform);
    			  }
    			catch (tf::TransformException ex){};
    			
			double x_vect_c = transform.getOrigin().x();
			double y_vect_c = transform.getOrigin().y();

			try{
      				listener.lookupTransform("/map", "/corner4",  
                               	ros::Time(0), transform);
    			   }
    			catch (tf::TransformException ex){};
    			
			double x_vect_d = transform.getOrigin().x();
			double y_vect_d = transform.getOrigin().y();

			//Find the minimum distance between the four line segments of the rectangle(wheelchair) and bed frame (line segment)
		
			//Line segment 1, between corner1 and corner2			
			double p1x = 12.78; 
			double p1y =  -0.2;
			double p2x = 12.78;
			double p2y = -1.2;
			double p3x =  x_vect_a;
			double p3y =  y_vect_a;
			double p4x =  x_vect_b;
			double p4y =  y_vect_b;
			double distanceSegment1; 
			DistanceFromSegment(p1x, p1y, p2x, p2y ,p3x, p3y, p4x, p4y, distanceSegment1);
			
			//Line segment 2, between corner2 and corner3		
			double distanceSegment2; 
			DistanceFromSegment(p1x, p1y, p2x, p2y ,x_vect_b, y_vect_b, x_vect_c, y_vect_c, distanceSegment2);

			//Line segment 3, between corner3 and corner4			
			double distanceSegment3; 
			DistanceFromSegment(p1x, p1y, p2x, p2y ,x_vect_c, y_vect_c, x_vect_d, y_vect_d, distanceSegment3);
			
			//Line segment 4, between corner4 and corner1			
			double distanceSegment4; 
			DistanceFromSegment(p1x, p1y, p2x, p2y ,x_vect_d, y_vect_d, x_vect_a, y_vect_a, distanceSegment4);
			
			//Find the minimum distance of all 8 line to segment distances
			double distarray [4]= {distanceSegment1, distanceSegment2, distanceSegment3, distanceSegment4};
			
			double mindistance = distarray [0];
			for (int i = 1; i < 4; i++)
				{
				if (distarray[i] < mindistance)
					{
				    	mindistance = distarray[i];
					}
				}
					
			line.points[1].x = 12.78; line.points[1].y =-0.2;
			line.points[0].x = 12.78; line.points[0].y =-1.2; line.points[0].z = line.points[1].z = 0;
			marker_pub.publish(line);
					
					if(vel<0.09)
					{
					 	if (mindistance < 1) 
							{
							ROS_INFO("Yaw: %f  Distance: %f",yaw*57.29 ,mindistance);
				       	       		}
					}



		while(!end_pose_rec && ros::ok()) ros::spinOnce();
			 //{
		//laser_pub.publish(laser_scan);
		dx = end_pose.position.x-start_pose.position.x;
  		dy = end_pose.position.y-start_pose.position.y;

 		double dist_tot = sqrt(dx*dx+dy*dy), time = (end_time-start_time).toSec(), avg_lin_v = dist_tot/time;

  		for(size_t i=0; i<yaws.size(); i++) sig_yaw += pow(yaws[i]-cma_yaw, 2);
  		std_dev_yaw = sqrt(sig_yaw/yaw_c)*57.3;
  		for(size_t i=0; i<vels.size(); i++) sig_lin_v += pow(vels[i]-cma_lin_v, 2);
 		 std_dev_lin_v = sqrt(sig_lin_v/lin_v_c);

 	 	ROS_INFO("Std dev --- Orientation: %f, Linear speed: %f", std_dev_yaw, std_dev_lin_v);
  		ROS_INFO("avarege linear velocity is %f, total dist %f", avg_lin_v, dist_tot);
  		ROS_INFO("Travelled %f seconds", time);
		//ROS_INFO("Minimal distance to any obstale %f meters", min_range);
		ros::spinOnce();
			//}

	rate.sleep();
	}
	
//}
//ros::spinOnce();	
return 0;
}





